// motors
const int E1 = 5; //M1 Speed Control
const int E2 = 6; //M2 Speed Control
const int M1 = 4; //M1 Direction Control
const int M2 = 7; //M2 Direction Control

// encoders
const int encoder_motor_right = 2;
const int encoder_motor_left = 3;
const float _mywheel_step = (PI*6.5f)/20.0f;
long _encoder_step_left=0;
long _encoder_step_right=0;

// IF Range sensor
const int infra_range_body_front = 1;

// IF Range Switch
const int switch_body_back_left = 11;
const int switch_body_back_center = 10;
const int switch_body_back_right = 9;

// body settings
const int body_ir_min = 10, body_ir_max = 80;

//////////////
// encoders
void reinit_encoders()
{
  _encoder_step_left = _encoder_step_right = 0;
}
void setup_encoders(void)
{
  pinMode(encoder_motor_right, INPUT);
  pinMode(encoder_motor_left, INPUT);
  reinit_encoders();
  attachInterrupt(0, read_encoder_right, CHANGE);
  attachInterrupt(1, read_encoder_left, CHANGE);  
}
void read_encoder_right(void)
{
  if(motor_step_counting)
  {
    _encoder_step_right++;
  }
}
void read_encoder_left(void)
{
  if(motor_step_counting)
  {
    _encoder_step_left++;
  }
}

///////////////
// motor
void motor_stop(void)
{
  digitalWrite(E1,LOW);
  digitalWrite(E2,LOW);
  motor_running = false;

  if(motor_step_counting)
  {
    motor_step_counting = false;
    body_lastmove_distance_left = _mywheel_step*(float)_encoder_step_left;
    body_lastmove_distance_right = _mywheel_step*(float)_encoder_step_right;
    reinit_encoders();
    send_body_moving_distance();
  }
}
void motor_action(const char a, const char b, const boolean a2, const boolean b2)
{
  motor_running = true;
  if(a2==b2)
    motor_step_counting = true;
  analogWrite (E1,a);
  digitalWrite(M1,a2?HIGH:LOW);
  analogWrite (E2,b);
  digitalWrite(M2,b2?HIGH:LOW);
}
void setup_motor(void)
{
  pinMode(E1, OUTPUT);
  pinMode(E2, OUTPUT);
  pinMode(M1, OUTPUT);
  pinMode(M2, OUTPUT);
}

int map_power_motor(int iPercent)
{
  return map(iPercent, 0, 100, 75, 250);
}

///////////////
// Range

int setup_rangeswiches(){
  pinMode(switch_body_back_left, INPUT);
  pinMode(switch_body_back_center, INPUT);  
  pinMode(switch_body_back_right, INPUT);
}

short get_range_body()
{
  return get_range_sensor_gp2ya21(infra_range_body_front, _beam_count, _beam_delay, _beam_precision, body_ir_min, body_ir_max);
}
